# 利用棋盘格进行相机标定
import cv2
import numpy as np
import glob  # 用于读取文件夹中文件

# 寻找棋盘格角点
# 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER,30,0.001);
# 棋盘格规格
w = 5
h = 5

# 世界坐标系中的棋盘格点
# 准备目标点，例如 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((w*h,3),np.float32)
# 将世界坐标系建在标定板上，所有Z坐标都为0
objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)

# 保存棋盘格中世界坐标和图像中的点
objpoints = [] #存储世界坐标系中的点
imagepoints = [] #存储图像中的点

images = glob.glob("E:\\VC_project\\figure\\chess1\\*.jpg")
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    #获取x，y坐标
    size = gray.shape[::-1]

    # 获取棋盘格角点
    ret,corners = cv2.findChessboardCorners(gray,(w,h))

    # 如果找到角点，添加3D点，2D点
    if ret:
        objpoints.append(objp)
        # 增加角点的准确度
        corner1 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imagepoints.append(corner1)

        # 画出并显示角点
        img = cv2.drawChessboardCorners(img,(w,h),corner1,ret)
        cv2.imshow("img",img)
        cv2.waitKey(500)
    cv2.imshow("gray",gray)
    cv2.waitKey(500)
# 相机标定

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imagepoints, size, None, None)
# 保存相机参数
np.savez('C.npz', mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
print("ret:", ret)
print("内参数矩阵:\n", mtx)
print("畸变系数:\n", dist)
print("旋转向量:", rvecs)  # 外参数
print("平移向量:", tvecs)  # 外参数


# 去畸变
img2 = cv2.imread("E:\\VC_project\\figure\\chess1\\_20190625191839.jpg")
h,w = img2.shape[:2]
newcameramtx,roi = cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),0,(w,h))
dst = cv2.undistort(img2,mtx,dist,None,newcameramtx)

# 根据前面ROI区域裁剪图片
cv2.imshow("calibresult",dst)

# 计算反投影误差
total_error = 0
for i in range(len(objpoints)):
    imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(imagepoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
    total_error += error
print("mena error:",total_error/len(objpoints))

cv2.waitKey()
